/*
 *  pixelformat.c
 *  libCV
 *
 *  Created by Gregory Koch on 16/01/11.
 *  Copyright 2011 IFlyBotIV. All rights reserved.
 *
 */

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include "ccvt.h"

hsv_pixel_t ccvt_rgb24_hsv(unsigned char r, unsigned char g, unsigned char b)
{
    hsv_pixel_t hsv;
	unsigned char rgb_min, rgb_max;
	rgb_min = MIN3(r, g, b);
	rgb_max = MAX3(r, g, b);
	
	hsv.val = rgb_max;
	if (hsv.val == 0) 
	{
		hsv.hue = hsv.sat = 0;
		return hsv;
	}
	
	hsv.sat = 255*(rgb_max - rgb_min)/hsv.val;
	if (hsv.sat == 0) 
	{
		hsv.hue = 0;
		return hsv;
	}
	
	/* Compute hue */
	if (rgb_max == r) 
	{
		hsv.hue = 0 + 43*(g - b)/(rgb_max - rgb_min);
	} 
	else if (rgb_max == g) 
	{
		hsv.hue = 85 + 43*(b - r)/(rgb_max - rgb_min);
	} 
	else /* rgb_max == rgb.b */ 
	{
		hsv.hue = 171 + 43*(r - g)/(rgb_max - rgb_min);
	}
	
	return hsv;
}


void ccvt_420p_rgb24(int width, int height, const unsigned char *src, unsigned char *dst)
{
	int line, col, linewidth;
	int y, u, v, yy, vr, ug, vg, ub;
	int r, g, b;
	const unsigned char *py, *pu, *pv;
    
	linewidth = width >> 1;
	py = src;
	pu = py + (width * height);
	pv = pu + (width * height) / 4;
    
	y = *py++;
	yy = y << 8;
	u = *pu - 128;
	ug =   88 * u;
	ub =  454 * u;
	v = *pv - 128;
	vg =  183 * v;
	vr =  359 * v;
    
	for (line = 0; line < height; line++) {
		for (col = 0; col < width; col++) {
			r = (yy +      vr) >> 8;
			g = (yy - ug - vg) >> 8;
			b = (yy + ub     ) >> 8;
            
			if (r < 0)   r = 0;
			if (r > 255) r = 255;
			if (g < 0)   g = 0;
			if (g > 255) g = 255;
			if (b < 0)   b = 0;
			if (b > 255) b = 255;
            
            
            *dst++ = r;
            *dst++ = g;
            *dst++ = b;
            
			
			
			y = *py++;
			yy = y << 8;
			if (col & 1) {
				pu++;
				pv++;
                
				u = *pu - 128;
				ug =   88 * u;
				ub =  454 * u;
				v = *pv - 128;
				vg =  183 * v;
				vr =  359 * v;
			}
		} /* ..for col */
		if ((line & 1) == 0) { 
			pu -= linewidth;
			pv -= linewidth;
		}
	} /* ..for line */
}


void ccvt_420p_grey8(int width, int height, const unsigned char *src, unsigned char *dst)
{
    memcpy(dst, src, width*height);
}

void ccvt_yuyv_rgb24(int width, int height, const unsigned char *src, unsigned char *dst)
{
    const unsigned char *s;
    unsigned char *d;
    int l, c;
    int r, g, b, cr, cg, cb, y1, y2;
    
    l = height;
    s = src;
    d = dst;
    while (l--) {
        c = width >> 1;
        while (c--) {
            y1 = *s++;
            cb = ((*s - 128) * 454) >> 8;
            cg = (*s++ - 128) * 88;
            y2 = *s++;
            cr = ((*s - 128) * 359) >> 8;
            cg = (cg + (*s++ - 128) * 183) >> 8;
            
            r = y1 + cr;
            b = y1 + cb;
            g = y1 - cg;
            SAT(r);
            SAT(g);
            SAT(b);
            
            *d++ = r;
            *d++ = g;
            *d++ = b;
            
            r = y2 + cr;
            b = y2 + cb;
            g = y2 - cg;
            SAT(r);
            SAT(g);
            SAT(b);
            
            *d++ = r;
            *d++ = g;
            *d++ = b;
        }
    }
}


void ccvt_yuyv_grey8(int width, int height, const unsigned char *src, unsigned char *dst)
{
    const unsigned char *s;
    unsigned char *d;
    int l, c;
    int y1, y2;
    
    l = height;
    s = src;
    d = dst;
    while (l--) {
        c = width >> 1;
        while (c--) {
            y1 = *s++;
            s++;
            y2 = *s++;
            s++;            
            *d++ = y1;
            *d++ = y2;
        }
    }   
}
